/**
 * @file
 * @brief Defines constants and utility function for canbus.
 */

#ifndef _WS_CANBUS_CONSTS_H_
#define _WS_CANBUS_CONSTS_H_

/**
 * @namespace canbus
 * @brief canbus
 */
namespace canbus {
    
  constexpr int kVehicleMaxSpeed = 40; // 40 percentage
  constexpr int kVehicleMaxAngle = 100; // 100 percentage 

  constexpr int kHoistHeightDefault = 10800;
  constexpr int kHydraulicDefault = 350; 
  constexpr int kHydraulicMax = 650;

  constexpr double Deg2Rad = M_PI/180;
  constexpr double Rad2Deg = 180/M_PI;

  constexpr double kCoefficient = 1612.09661836;
  constexpr double kIntercept = 3970.58293076;

  /**
   * @brief calculate the current value based on curve fitting 
   * @param input_val distance value
   * @return current value
   */
  template<typename T>
  inline double sensorDataFit(T input_val) {
    return input_val*kCoefficient+kIntercept;
  }

  /**
   * @brief calculate the distance value based on curve fitting 
   * @param input_val current value
   * @return distance value
   */
  template<typename T>
  inline double sensorDataFitInv(T input_val) {
    return (input_val-kIntercept)/kCoefficient;
  }

  /**
   * @brief clamp the data value
   * @param x_val data value to be clamped
   * @param min_val the lower bound
   * @param max_val the upper bound 
   * @return the value after clamped
   */
  template<typename T>
  T mathClamp(T x_val, T min_val, T max_val) {
    return std::max(min_val, std::min(x_val, max_val));
  }

}

#endif